%author: wxj233
%time: 2024.1.11 12:00
%function: 100次蒙特卡洛实验

clc;
clear;
clear Function;
close all;

% 3倍sigma点就是精度
p_d = 0.4/3;  % 测距精度0.4m
p_v = 0.02778/3;  % 测速精度0.0277m/s
p_theta = 0.3/180*pi/3; % 测角精度0.3°（近距离），远距离是0.1°。都是波束中心附近

% 初始化仿真器
% dt
dt = 0.072;

simulater = Simulater(dt);
% 产生第一个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster1_1 = simulater.generateFeaturePoints(10, 2.2, [0.5,0.5,0.5,0.5], 1);
cluster1_2 = simulater.generateFeaturePoints(10, 2.2, [0.5,0.5], 2);
% 根据特征点群，产生第一条轨迹
% cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
[T1_1_1, rT_1_1_1, ep1_1_1, ev1_1_1, et1_1_1] = simulater.generateTrack(cluster1_1, [-150, 105], [10, -10], [0, 1], 0, [0, 5;8,10], 1, p_d, p_v, p_theta, 11, 12);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
[T1_2_1, rT_1_2_1, ep1_2_1, ev1_2_1, et1_2_1] = simulater.generateTrack(cluster1_2, [-150, 105], [10, -10], [0, 1], 0, [0, 10], 1, p_d, p_v, p_theta, 21, 22);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

[T1_1_2, rT_1_1_2, ep1_1_2, ev1_1_2, et1_1_2] = simulater.generateTrack(cluster1_1, ep1_1_1, ev1_1_1, [0, 0], et1_1_1, [dt,4; 7,10], 1, p_d, p_v, p_theta, 31, 32);
[T1_2_2, rT_1_2_2, ep1_2_2, ev1_2_2, et1_2_2] = simulater.generateTrack(cluster1_2, ep1_2_1, ev1_2_1, [0, 0], et1_2_1, [dt, 10], 1, p_d, p_v, p_theta, 46, 47);

[T1_1_3, rT_1_1_3, ep1_1_3, ev1_1_3, et1_1_3] = simulater.generateTrack(cluster1_1, ep1_1_2, ev1_1_2, [0, -1], et1_1_2, [dt, 10], 1, p_d, p_v, p_theta, 36, 37);
[T1_2_3, rT_1_2_3, ep1_2_3, ev1_2_3, et1_2_3] = simulater.generateTrack(cluster1_2, ep1_2_2, ev1_2_2, [0, -1], et1_2_2, [dt,dt;dt*50, 10], 1, p_d, p_v, p_theta, 41, 42);

rT = [(rT_1_1_1*4+rT_1_2_1*2)/6; (rT_1_1_2*4+rT_1_2_2*2)/6; (rT_1_1_3*4+rT_1_2_3*2)/6];

clusterTracer = ClusterTracer(40, p_d^2, p_v^2, p_theta^2, 0.1, 1, 0.95, 1, 10, 5, 15, 3);
mfpTracer = MFPTracer(40, p_d^2, p_v^2, p_theta^2, 0.1, 1, 0.95, 0.8, 0.80, 60, 5, 15, 3);
[frame, isNextFrame]= simulater.getNextFrame();
frames = [];
while isNextFrame
    frame_c = clusterTracer.preproccess_cluster(frame, 20, 2, 1, 1, 10);  % 预处理points, epsilon, minpts, zoomX, zoomY, zoomV
    frame = mfpTracer.preproccess(frame, 10, 2, 1, 1, 10);  % 预处理points, epsilon, minpts, zoomX, zoomY, zoomV
    frames = [frames; frame];  % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]

    clusterTracer.tracer(frame_c);
    mfpTracer.tracer(frame);
    [frame, isNextFrame]= simulater.getNextFrame();
end
clusterTracer.finish();
mfpTracer.finish();



fp = clusterTracer.deadFps(1);
points = fp.points(:, [1, 13,15]);

points(:,1) = round(points(:,1), 3);
rT(:,1) = round(rT(:,1), 3);
Ic = ismember(rT(:,1), points(:, 1));
x = points(:,2)-rT(Ic, 2);
y = points(:,3)-rT(Ic, 3);
error_clu = sqrt(x.*x+y.*y);


for track = mfpTracer.deadTracks  % [1t, 2x, 3vx, 4y,5vy; ...]
    if size(track.points,1) > 100
        Ic = ismember(rT(:,1), track.points(:,1));
        x = track.points(:,2)-rT(Ic, 2);
        y = track.points(:,4)-rT(Ic, 3);
        error_ET = sqrt(x.*x+y.*y);
        points_st = track.points;
    end
end


figure(1)  % 跟踪轨迹图
plot(rT(:, 2), rT(:, 3), 'DisplayName', "真实轨迹");hold on;
plot(points(:,2), points(:,3), 'DisplayName', "聚类中心方法");hold on;
plot(points_st(:,2), points_st(:, 4),'DisplayName', "ET-SC");hold on;
title("遮蔽情况下的跟踪轨迹");
xlabel("x(m)");
ylabel("y(m)");
legend;


figure(2)  % 跟踪差误图
plot(points(:,1), error_clu, 'DisplayName', "聚类中心方法");hold on;
plot(track.points(:,1), error_ET,'DisplayName', "ET-SC");hold on;
title("遮蔽情况下的跟踪误差");
xlabel("t(s)");
ylabel("误差(m)");
legend;


